#include <iostream>
#include <vector>
#include <algorithm>
#include <deque>
#include <array>
#include "../../include/api/grid_map.h"
std::vector<std::vector<int>> bresenham(std::vector<int>& start, std::vector<int>& end) {
    // Setup initial conditions
    int x1 = start[0], y1 = start[1];
    int x2 = end[0], y2 = end[1];
    int dx = x2 - x1;
    int dy = y2 - y1;
    bool is_steep = abs(dy) > abs(dx);

    // Determine how steep the line is
    if (is_steep) {
        std::swap(x1, y1);
        std::swap(x2, y2);
    }

    bool swapped = false;

    // Swap start and end points if necessary and store swap state
    if (x1 > x2) {
        std::swap(x1, x2);
        std::swap(y1, y2);
        swapped = true;
    }

    dx = x2 - x1;
    dy = y2 - y1;
    int error = dx / 2;
    int ystep = (y1 < y2) ? 1 : -1;

    // Iterate over bounding box generating points between start and end
    int y = y1;
    std::vector<std::vector<int>> points;

    for (int x = x1; x <= x2; ++x) {
        std::vector<int> coord = (is_steep) ? std::vector<int>{y, x} : std::vector<int>{x, y};
        points.push_back(coord);

        error -= abs(dy);

        if (error < 0) {
            y += ystep;
            error += dx;
        }
    }

    // Reverse the list if the coordinates were swapped
    if (swapped) {
        std::reverse(points.begin(), points.end());
    }

    return points;
}





std::vector<std::vector<int>> flood_fill(std::vector<int>& cpoint, std::vector<std::vector<int>>& pmap) {
    int sx = pmap.size();
    int sy = pmap[0].size();

    std::deque<std::vector<int>> fringe;
    fringe.push_front(cpoint);

    while (!fringe.empty()) {
        std::vector<int> n = fringe.back();
        fringe.pop_back();

        int nx = n[0];
        int ny = n[1];

        // West
        if (nx > 0) {
            if (pmap[nx - 1][ny] == 1.0) {
                pmap[nx - 1][ny] = 0.0;
                fringe.push_front({nx - 1, ny});
            }
        }

        // East
        if (nx < sx - 1) {
            if (pmap[nx + 1][ny] == 1.0) {
                pmap[nx + 1][ny] = 0.0;
                fringe.push_front({nx + 1, ny});
            }
        }

        // North
        if (ny > 0) {
            if (pmap[nx][ny - 1] == 1.0) {
                pmap[nx][ny - 1] = 0.0;
                fringe.push_front({nx, ny - 1});
            }
        }

        // South
        if (ny < sy - 1) {
            if (pmap[nx][ny + 1] == 1.0) {
                pmap[nx][ny + 1] = 0.0;
                fringe.push_front({nx, ny + 1});
            }
        }
    }

    return pmap;
}




GridMap::GridMap(std::vector<std::vector<double>>& polygonVertices, std::vector<std::vector<double>>& pointInsidePolygon) {
    map_center[0] = 0.0;
    map_center[1] = 0.0;
    map_width_m = 9.0;
    map_length_m = 9.0;
    width = 10;
    height = 10;
    map_resolution_m = 0.01; // [m]
    flight_area_vertices = polygonVertices;
    create_borders_grid_map(pointInsidePolygon);    
}



void GridMap::create_borders_grid_map(std::vector<std::vector<double>>& polygon_center) {
    int WIDTH = static_cast<int>(map_width_m / map_resolution_m);
    int LENGTH = static_cast<int>(map_length_m / map_resolution_m);
    width = WIDTH;
    height = LENGTH;

    gmap.assign(WIDTH, std::vector<int>(LENGTH, 1));

    std::vector<std::vector<int>> points = meters2grid(flight_area_vertices);

    // Append the first point to the end to close the loop
    points.push_back(points[0]);

    for (size_t i = 0; i < points.size() - 1; ++i) {
        std::vector<std::vector<int>> line = bresenham(points[i], points[i + 1]);
        for (auto& l : line) {
            gmap[l[1]][l[0]] = 0;
        }
    }
    
    std::vector<int> polygon_center_grid = meters2grid(polygon_center)[0];
    flood_fill(polygon_center_grid, gmap);
    
    // Now gmap contains the desired result
    // You can access it using gmap[i][j]
}





std::vector<std::vector<int>> GridMap::add_obstacles_to_grid_map(std::vector<std::vector<std::vector<double>>>& obstacles) {
    for (auto& obstacle : obstacles) {
        std::vector<std::vector<int>>  x1 = meters2grid(obstacle);
        std::vector<std::vector<int>>  x2 = meters2grid(obstacle);
        std::vector<std::vector<int>>  y1 = meters2grid(obstacle);
        std::vector<std::vector<int>>  y2 = meters2grid(obstacle);

        /*if (x1 > x2) {
            std::swap(x1, x2);
        }

        if (y1 > y2) {
            std::swap(y1, y2);
        }

        for (int i = x1; i < x2; ++i) {
            for (int j = y1; j < y2; ++j) {
                gmap[i][j] = 1;
            }
        }*/
    }

    return gmap;
}
std::vector<std::vector<int>> GridMap::meters2grid(std::vector<std::vector<double>>& pose_m) {
    std::vector<std::vector<int>> result;

    for (auto& pose : pose_m) {
        std::vector<int> grid_point = {
            static_cast<int>(pose[0] / map_resolution_m),
            static_cast<int>(pose[1] / map_resolution_m)
        };
        result.push_back(grid_point);
    }

    return result;
}
std::vector<std::vector<int>> GridMap::grid2meters(std::vector<std::vector<double>>& pose_grid) {
    int ncols = static_cast<int>(map_width_m / map_resolution_m);
    int nrows = static_cast<int>(map_length_m / map_resolution_m);

    std::vector<std::vector<int>> pose_meters;

    for (auto& row : pose_grid) {
        std::vector<int> row_meters;
        if (row.size() == 1) {
            row_meters.push_back((row[0] - ncols / 2) * map_resolution_m);
        } else {
            row_meters.push_back((row[0] - ncols / 2) * map_resolution_m - map_center[0]);
            row_meters.push_back((row[1] - nrows / 2) * map_resolution_m - map_center[1]);
        }
        pose_meters.push_back(row_meters);
    }

    return pose_meters;
}

